/*
*
==============================================================================
*  Name        : 
*  Part of     : Omen Game Engine
*  Description : 
*  Version     : 1.0
*
*  Copyright (c) 2013 Lauri Kortevaara.
* 
==============================================================================
*/

#define OMEN_SOURCE 1

#include "OmenCamera.h"
#include <OmenMath.h>
#include <OmenUtils.h>
#include <IOmenFactory.h>

#include <iostream>
#include <iomanip>
#include <string>
#include <sstream>

using namespace Omen;

///
/// <summary>The Camera Object constructor</summary>
///
Camera::Camera( const Omen::Vector3d& position, const Omen::Vector3d& right, const Omen::Vector3d& up, const Omen::Vector3d& lookAt ) :
    m_projectionType( ICamera::ProjectionType_Perspective),
    m_fieldOfView(90.0),            // 45 degree viewing angle
    m_nearPlane(0.01f),            // The near plane in 0.001 units away from the eye to the positive z direction
    m_farPlane(1000.0f),             // The far plane is 100.0 units away from the eye to the positive z direction
    m_currentVelocity(.0f,.0f,.0f),  // Initial velocity is zero
    m_acceleration( 180.0f, 180.0f, 180.0f ), // Initial acceleration of camera
    m_maxVelocity( 1.0f, 1.0f, 1.0f ),  // The initial velocity of camera
    m_rotationSpeed( DEFAULT_ROTATION_SPEED )
{
    // Set the position and viewing angles
    m_worldPosition = position;
    m_yaw = m_pitch = m_roll = 0.0f;
    
    // Set the orthogonal rotation axes
    m_right =   right;
    m_up =      up;
    m_forward = lookAt;

    // Register this as an event handler
    GetOmenFactory().getKernel().registerEventHandler(this);
    // Register event handlers
    registerEventFunc( this, &Camera::OnCollisionEvent );

    Omen::IKernel& kernel = GetOmenFactory().getKernel();
    Omen::Float aspectRatio = kernel.getProperty( L"window_aspect_ratio" );
    setPerspective( m_fieldOfView, aspectRatio, m_nearPlane, m_farPlane );
}

void Camera::setPerspective( Omen::Float fov, Omen::Float aspect, Omen::Float znear, Omen::Float zfar )
{
    m_aspectRatio = aspect;

    // Set the projection matrix
    m_scaleX = m_scaleY = m_scaleZ = 1.0f;

    
    Omen::Float f = 1.0f / tanf((fov*Omen::PI_OVER_180)/2.0f);
    Omen::Float aspectInv = 1.0f / aspect;

    // First column
    m_projectionMatrix.m11 = f*aspectInv;
    m_projectionMatrix.m12 = 0.0;
    m_projectionMatrix.m13 = 0.0;
    m_projectionMatrix.m14 = 0.0;

    // Second column
    m_projectionMatrix.m21 = 0.0;
    m_projectionMatrix.m22 = f;
    m_projectionMatrix.m23 = 0.0;
    m_projectionMatrix.m24 = 0.0;

    // Third column
    m_projectionMatrix.m31 = 0.0;
    m_projectionMatrix.m32 = 0.0;
    m_projectionMatrix.m33 = (zfar)/(zfar-znear);
    m_projectionMatrix.m34 = 1.0f;

    // Fourth column
    m_projectionMatrix.m41 = 0.0;
    m_projectionMatrix.m42 = 0.0;
    m_projectionMatrix.m43 = -(znear*zfar)/(zfar-znear);
    m_projectionMatrix.m44 = 0.0;
}

void Camera::setOrtho( Omen::Float left, Omen::Float right, Omen::Float bottom, Omen::Float top, Omen::Float nearVal, Omen::Float farVal )
{
      // First column
    m_projectionMatrix[0][0] = 2.0f / (right-left);
    m_projectionMatrix[0][1] = 0.0;
    m_projectionMatrix[0][2] = 0.0;
    m_projectionMatrix[0][3] = 0.0;

    // Second column
    m_projectionMatrix[1][0] = 0.0;
    m_projectionMatrix[1][1] = 2.0f / (top-bottom);
    m_projectionMatrix[1][2] = 0.0;
    m_projectionMatrix[1][3] = 0.0;

    // Third column
    m_projectionMatrix[2][0] = 0.0;
    m_projectionMatrix[2][1] = 0.0;
    m_projectionMatrix[2][2] = 1.0f / (farVal-nearVal);
    m_projectionMatrix[2][3] = nearVal / (nearVal-farVal);

    // Fourth column
    m_projectionMatrix[3][0] = 0.0;
    m_projectionMatrix[3][1] = 0.0;
    m_projectionMatrix[3][2] = 0.0;
    m_projectionMatrix[3][3] = 1.0;
}


///
/// <summary>The Camera Object destructor</summary>
///
Camera::~Camera()
{
}

///
/// 
///
Omen::Vector3d    Camera::position() const
{
    return m_worldPosition;
}

///
/// <summary> Returns the normalized direction vector pointing right </summary>
///
Omen::Vector3d    Camera::right() const
{
    return m_right;
}

///
/// <summary> Returns the normalized direction vector pointing up </summary>
///
Omen::Vector3d    Camera::up() const
{
    return m_up;
}

///
/// <summary> Returns the normalized direction vector pointing forward</summary>
///
Omen::Vector3d    Camera::forward() const
{
    return m_forward;
}

///
/// <summary>Set the poisition of the camera in world coorinates</summary>
///
void  Camera::setPosition(const Omen::Vector3d& position)
{
    m_worldPosition = position;
    m_transform.setPosition(position);
    updateCameraMatrix();
}

///
/// <summary> The vector v is the target coordinate where camera is looking at</summary>
///
void  Camera::setLookAt(const Omen::Vector3d& target)
{
    m_forward = m_worldPosition - target;
    m_forward.normalize();

    m_right = Vector3d::crossProduct( m_up, m_forward );
}

///
/// <summary>Set the up direction vector of the camera's tranfromation matrix</summary>
///
void  Camera::setRight(const Omen::Vector3d& right)
{
    m_right = right.normalized();
    Omen::Float yaw = acos(Omen::Vector3d::dotProduct(m_right, X_AXIS)/(m_right.length()*X_AXIS.length()));
    m_yaw = yaw;
}

///
/// <summary>Set the up direction vector of the camera's tranfromation matrix</summary>
///
void  Camera::setUp(const Omen::Vector3d& up)
{
    m_up = up.normalized();
}

///
/// <summary>Set the up direction vector of the camera's tranfromation matrix</summary>
///
void  Camera::setForward(const Omen::Vector3d& forward)
{
    m_forward = forward.normalized();
}

///
/// <summary> The angle in degrees around the x-axis, turning from from down-to-up e.g. clock-wise</summary>
///
Omen::Float Camera::pitch() const 
{
    return m_pitch;
}

///
/// <summary> The angle in degres around the y-axis, turning from right-to-left e.g. clock-wise</summary>
///
Omen::Float Camera::yaw() const 
{
    return m_yaw;
}

///
/// <summary> The angle in degres around the z-axis, turning clock-wise</summary>
///
Omen::Float Camera::roll() const 
{
    return m_roll;
}

///
/// <summary> The angle in degrees around the x-axis, turning from from down-to-up e.g. clock-wise</summary>
///
void Camera::setPitch( Omen::Float pitch) 
{
    m_pitch = pitch;
    m_transform.setRotation( Vector3d(m_pitch, m_yaw, m_roll) );
}

///
/// <summary> The angle in degres around the y-axis, turning from right-to-left e.g. clock-wise</summary>
///
void Camera::setYaw( Omen::Float yaw) 
{
    m_yaw = yaw;
    m_transform.setRotation( Vector3d(m_pitch, m_yaw, m_roll) );
}

///
/// <summary>Set's the velocity of the camera</summary>
///
void Camera::setVelocity( const Omen::Vector3d& velocity )
{
    m_currentVelocity = velocity;
}

///
/// <summary>Set's the velocity of the camera</summary>
///
void Camera::setMaxVelocity( const Omen::Vector3d& velocity )
{
    m_maxVelocity = velocity;
}

///
/// <summary>Set's the acceleration of the camera</summary>
///
void Camera::setAcceleration( const Omen::Vector3d& acceleration )
{
    m_acceleration = acceleration;
}

///
/// <summary>Returns the velocity of the camera </summary>
///
Omen::Vector3d Camera::velocity() const
{
    return m_currentVelocity;
}

///
/// <summary>Returns the velocity of the camera </summary>
///
Omen::Vector3d Camera::maxVelocity() const
{
    return m_maxVelocity;
}

///
/// <summary>Returns the acceleration of the camera</summary>
///
Omen::Vector3d Camera::acceleration() const
{
    return m_acceleration;
}

///
/// <summary> Returns the rotation speed of the camera </summary>
///
 Omen::Float Camera::rotationSpeed() const
 {
     return m_rotationSpeed;
 }

 /// <summary> Sets the rotation speed of the camera </summary>
 void Camera::setRotationSpeed( Omen::Float speed )
 {
     m_rotationSpeed = speed;
 }

Omen::Vector3d Camera::futurePosition( const Omen::Vector3d& direction, Omen::Float elapsedTime )
{
        // Update the camera's position only if it's moving
    if( m_currentVelocity.squaredLength() != 0.0f )
    {
        Vector3d displacement = (m_currentVelocity * elapsedTime ) + (m_acceleration * elapsedTime * elapsedTime * 0.5f);

        // Floating point rounding errors will slowly accumulate and cause the
        // camera to move along each axis. To prevent any unintended movement
        // the displacement vector is clamped to zero for each direction that
        // the camera isn't moving in. Note that the updateVelocity() method
        // will slowly decelerate the camera's velocity back to a stationary
        // state when the camera is no longer moving along that direction. To
        // account for this the camera's current velocity is also checked.

        if (direction.x == 0.0f && fabsf(m_currentVelocity.x) < 1e-6f)
            displacement.x = 0.0f;

        if (direction.y == 0.0f && fabsf(m_currentVelocity.y) < 1e-6f)
            displacement.y = 0.0f;

        if (direction.z == 0.0f && fabsf(m_currentVelocity.z) < 1e-6f)
            displacement.z = 0.0f;

        // Moves the camera by dx world units to the left or right; dy
        // world units upwards or downwards; and dz world units forwards
        // or backwards.
        Omen::Vector3d eye = m_worldPosition;
    
        Omen::Vector3d forward = m_forward*displacement.z;
        Omen::Vector3d right = m_right*displacement.x;
        Omen::Vector3d up = m_up*displacement.y;

        // Calculate the forward direction. Can't just use the camera's local
        // z axis as doing so will cause the camera to move more slowly as the
        // camera's view approaches 90 degrees straight up and down.
        /*forward = Omen::Vector3d::crossProduct( m_right, Y_AXIS);
        forward.normalize();*/

        eye += right;
        eye += up;
        eye += forward;
        return eye;
    }
    return this->m_worldPosition;
}

void Camera::updatePosition( const Omen::Vector3d& direction, Omen::Float elapsedTime )
{
    m_movementDirection = direction;
    // Update the camera's position only if it's moving
    if( m_currentVelocity.squaredLength() != 0.0f )
    {
        Vector3d displacement = (m_currentVelocity * elapsedTime ) + (m_acceleration * elapsedTime * elapsedTime * 0.5f);

        // Floating point rounding errors will slowly accumulate and cause the
        // camera to move along each axis. To prevent any unintended movement
        // the displacement vector is clamped to zero for each direction that
        // the camera isn't moving in. Note that the updateVelocity() method
        // will slowly decelerate the camera's velocity back to a stationary
        // state when the camera is no longer moving along that direction. To
        // account for this the camera's current velocity is also checked.

        if (direction.x == 0.0f && fabsf(m_currentVelocity.x) < 1e-6f)
            displacement.x = 0.0f;

        if (direction.y == 0.0f && fabsf(m_currentVelocity.y) < 1e-6f)
            displacement.y = 0.0f;

        if (direction.z == 0.0f && fabsf(m_currentVelocity.z) < 1e-6f)
            displacement.z = 0.0f;

        move(displacement.x, displacement.y, displacement.z);
    }

    // Continuously update the camera's velocity vector even if the camera
    // hasn't moved during this call. When the camera is no longer being moved
    // the camera is decelerating back to its stationary state.
    updateVelocity(direction,elapsedTime);
}

///
/// <summary>Updates the velocity so that it decelerates to zero
///
void Camera::updateVelocity( const Omen::Vector3d& direction, Omen::Float elapsedTime)
{
    // Updates the camera's velocity based on the supplied movement direction
    // and the elapsed time (since this method was last called). The movement
    // direction is the in the range [-1,1].
    if (direction.x != 0.0f)
    {
        // Camera is moving along the x axis.
        // Linearly accelerate up to the camera's max speed.

        m_currentVelocity.x += direction.x * m_acceleration.x * elapsedTime;

        if (m_currentVelocity.x > m_maxVelocity.x)
            m_currentVelocity.x = m_maxVelocity.x;
        else 
        if (m_currentVelocity.x < -m_maxVelocity.x)
            m_currentVelocity.x = -m_maxVelocity.x;
    }
    else
    {
        // Camera is no longer moving along the x axis.
        // Linearly decelerate back to stationary state.

        if (m_currentVelocity.x > 0.0f)
        {
            if ((m_currentVelocity.x -= m_acceleration.x * elapsedTime) < 0.0f)
                m_currentVelocity.x = 0.0f;
        }
        else
        {
            if ((m_currentVelocity.x += m_acceleration.x * elapsedTime) > 0.0f)
                m_currentVelocity.x = 0.0f;
        }
    }

    if (direction.y != 0.0f)
    {
        // Camera is moving along the y axis.
        // Linearly accelerate up to the camera's max speed.

        m_currentVelocity.y += direction.y * m_acceleration.y * elapsedTime;

        if (m_currentVelocity.y > m_maxVelocity.y)
            m_currentVelocity.y = m_maxVelocity.y;
        else if (m_currentVelocity.y < -m_maxVelocity.y)
            m_currentVelocity.y = -m_maxVelocity.y;
    }
    else
    {
        // Camera is no longer moving along the y axis.
        // Linearly decelerate back to stationary state.

        if (m_currentVelocity.y > 0.0f)
        {
            if ((m_currentVelocity.y -= m_acceleration.y * elapsedTime) < 0.0f)
                m_currentVelocity.y = 0.0f;
        }
        else
        {
            if ((m_currentVelocity.y += m_acceleration.y * elapsedTime) > 0.0f)
                m_currentVelocity.y = 0.0f;
        }
    }

    if (direction.z != 0.0f)
    {
        // Camera is moving along the z axis.
        // Linearly accelerate up to the camera's max speed.
       m_currentVelocity.z += direction.z * m_acceleration.z * elapsedTime;
       if (m_currentVelocity.z > m_maxVelocity.z)
           m_currentVelocity.z = m_maxVelocity.z;
        else if (m_currentVelocity.z < -m_maxVelocity.z)
            m_currentVelocity.z = -m_maxVelocity.z;
    }
    else
    {
        // Camera is no longer moving along the z axis.
        // Linearly decelerate back to stationary state.
        if (m_currentVelocity.z > 0.0f)
        {
            if ((m_currentVelocity.z -= m_acceleration.z * elapsedTime) < 0.0f)
                m_currentVelocity.z = 0.0f;
        }
        else
        {
            if ((m_currentVelocity.z += m_acceleration.z * elapsedTime) > 0.0f)
                m_currentVelocity.z = 0.0f;
        }
    }
}

void Camera::move(Omen::Float dx, Omen::Float dy, Omen::Float dz)
{
    // Moves the camera by dx world units to the left or right; dy
    // world units upwards or downwards; and dz world units forwards
    // or backwards.
    Omen::Vector3d eye = m_worldPosition;
    
    Omen::Vector3d forward = m_forward*dz;
    Omen::Vector3d right = m_right*dx;
    Omen::Vector3d up = m_up*dy;

    // Calculate the forward direction. Can't just use the camera's local
    // z axis as doing so will cause the camera to move more slowly as the
    // camera's view approaches 90 degrees straight up and down.
    /*forward = Omen::Vector3d::crossProduct( m_right, Y_AXIS);
    forward.normalize();*/

    eye += right;
    eye += up;
    eye += forward;

    setPosition(eye);
}


void Camera::updateCameraMatrix()
{
    Matrix4x4 rotMatrix;
    rotMatrix.rotateY( m_yaw );

    Vector3d right   = rotMatrix * X_AXIS;
    Vector3d up      = rotMatrix * Y_AXIS;
    Vector3d forward = rotMatrix * Z_AXIS;

    bool bOrthogonalize = true;
    if( bOrthogonalize )
    {
        forward.normalize();
        up =  Vector3d::crossProduct(forward,right);
        up.normalize();
        right = Vector3d::crossProduct(up,forward);
        right.normalize();
        
        m_right = right;
        m_up = up;
        m_forward = forward;
    }
    
    m_viewMatrix[0][0] = right.x;
    m_viewMatrix[0][1] = right.y;
    m_viewMatrix[0][2] = right.z;
    m_viewMatrix[0][3] = -Vector3d::dotProduct(m_right, m_worldPosition);

    m_viewMatrix[1][0] = up.x;
    m_viewMatrix[1][1] = up.y;
    m_viewMatrix[1][2] = up.z;
    m_viewMatrix[1][3] = -Vector3d::dotProduct(m_up, m_worldPosition);

   m_viewMatrix[2][0] = forward.x;
   m_viewMatrix[2][1] = forward.y;
   m_viewMatrix[2][2] = forward.z;
   m_viewMatrix[2][3] = -Vector3d::dotProduct(m_forward,m_worldPosition);

    m_viewMatrix[3][0] = 0;
    m_viewMatrix[3][1] = 0;
    m_viewMatrix[3][2] = 0;
    m_viewMatrix[3][3] = 1.0f;

    m_viewMatrix = m_transform.transformation();
    m_rotationMatrix = m_viewMatrix;
    //m_rotationMatrix.setRow( 4, Vector3d(0.0,0.0,1.0) );
}

///
/// <summary> The angle in degres around the z-axis, turning clock-wise</summary>
///
void Camera::setRoll(Omen::Float roll) 
{
    m_roll = roll * Omen::PI_OVER_180;
    m_transform.setRotation( Vector3d(m_pitch, m_yaw, m_roll) );
}

///
/// <summary> Sets the view matrix for this camera </summary>
///
void Camera::setViewMatrix( const Omen::Matrix4x4& viewMatrix ) 
{
    m_viewMatrix = viewMatrix;
} 

///
/// <summary> Get the view matrix from the camera </summary>
///
void Camera::getViewMatrix( Omen::Matrix4x4& viewMatrix ) 
{
    Omen::Matrix4x4 rotationMatrix;
    rotationMatrix.setYawPitchRoll( m_yaw, m_pitch, m_roll );

    Omen::Vector3d right = rotationMatrix.column(0);
    Omen::Vector3d up = rotationMatrix.column(1);
    Omen::Vector3d forward = rotationMatrix.column(2);

    
    m_viewMatrix = rotationMatrix;
    m_viewMatrix.setColumn( 3, m_worldPosition );
    
    // Store the direction vectors
    Vector3d lookAt = m_worldPosition + m_forward;
    viewMatrix = m_viewMatrix;
    viewMatrix = m_transform.transformation();
}

///
/// <summary>Sets the projection matrix for this camera </summary>
///
void Camera::setProjectionMatrix( const Omen::Matrix4x4& projectionMatrix )
{
    m_projectionMatrix = projectionMatrix;
}


///
/// <summary>Sets the projection matrix for this camera </summary>
///
Camera::ProjectionType Camera::projectionType() const
{
    return m_projectionType;
}

///
/// <summary>Get the projection matrix from this camera</summary>
///
void Camera::getProjectionMatrix( Omen::Matrix4x4& projectionMatrix )
{
    if( this->m_projectionType == Camera::ProjectionType_Perspective )
      setPerspective( m_fieldOfView, m_aspectRatio, m_nearPlane, m_farPlane );
    else
      setOrtho( -m_viewWidth/2.0f, m_viewWidth/2.0f, m_viewHeight/2.0f, -m_viewHeight/2.0f, m_nearPlane, m_farPlane );
    projectionMatrix = m_projectionMatrix;
}


void Camera::setProperty( const std::wstring& propertyName, const std::wstring& propertyValue )
{
    IObject::setProperty(propertyName, propertyValue);

    if( propertyName == L"position" )
    {
        std::vector<Omen::Float> v;
        getFloatsVector( propertyValue, v );
        if( v.size() == 3 )
        {
            setPosition( Vector3d( v[0], v[1], v[2] ) );
        }
    }

    if( propertyName == L"right" )
    {
        std::vector<Omen::Float> v;
        getFloatsVector( propertyValue, v );
        if( v.size() == 3 )
        {
            setRight( Vector3d( v[0], v[1], v[2] ) );
        }
    }

    if( propertyName == L"up" )
    {
        std::vector<Omen::Float> v;
        getFloatsVector( propertyValue, v );
        if( v.size() == 3 )
        {
            setUp( Vector3d( v[0], v[1], v[2] ) );
        }
    }

    if( propertyName == L"forward" )
    {
        std::vector<Omen::Float> v;
        getFloatsVector( propertyValue, v );
        if( v.size() == 3 )
        {
            setForward( Vector3d( v[0], v[1], v[2] ) );
        }
    }

    if( propertyName == L"field-of-view" )
    {
        std::wstringstream ss;
        ss << propertyValue;
        Omen::Float fov;
        ss >> fov;
        this->m_fieldOfView = fov;
    }

    if( propertyName == L"view-width" )
    {
        std::wstringstream ss;
        ss << propertyValue;
        Omen::Float width;
        ss >> width;
        this->m_viewWidth = width;
    }

    if( propertyName == L"view-height" )
    {
        std::wstringstream ss;
        ss << propertyValue;
        Omen::Float height;
        ss >> height;
        this->m_viewHeight = height;
    }

    if( propertyName == L"projection" )
    {
        if( propertyValue == L"perspective" )
        {
            m_projectionType = Camera::ProjectionType_Perspective;
            Omen::IKernel& kernel = GetOmenFactory().getKernel();
            Omen::Float aspectRatio = kernel.getProperty( L"window_aspect_ratio" );
            setPerspective( m_fieldOfView, aspectRatio, m_nearPlane, m_farPlane );
        }
        if( propertyValue == L"orthographic" )
        {
            m_projectionType = Camera::ProjectionType_Orthographic;
            setOrtho( -m_viewWidth/2.0f, m_viewWidth/2.0f, m_viewHeight/2.0f, -m_viewHeight/2.0f, m_nearPlane, m_farPlane );
        }
    }
}

Omen::IBoundingVolume* Camera::boundingVolume( Omen::Float dTime )
{
    if( dTime != .0f )
        m_boundingSphere.setPosition( futurePosition(m_movementDirection, dTime) ); // Get position after dT seconds
    else
        m_boundingSphere.setPosition( position() ); // Get position now
    m_boundingSphere.setRadius( 1.0f );
    return &m_boundingSphere;
}

void Camera::OnCollisionEvent( const Omen::CollisionEvent* event )
{
    if( event->collider() == this )
    {
        Omen::Vector3d velocity = m_currentVelocity;
        velocity = m_viewMatrix * velocity;
        if( m_currentVelocity.y == 0.0f )
            velocity.y = 0;
        
        Omen::Vector3d direction = velocity.normalized();
        Omen::Vector3d planeNormal = event->collisionPlane().normal();

        std::cout << "Collisionplane: " << event->collisionPlane().a << ", " << event->collisionPlane().b << ", " << event->collisionPlane().c << ", " << event->collisionPlane().d << std::endl;
        std::cout << "CurrentVelocity: " << velocity.x << ", " << velocity.y << ", " << velocity.z << std::endl;

        if( Vector3d::dotProduct(direction, planeNormal ) < 0 )
        {
            Omen::Vector3d impulse = Omen::Vector3d::crossProduct( velocity, planeNormal );
            impulse = Omen::Vector3d::crossProduct( planeNormal, impulse );
            Omen::Vector3d newVelocity = impulse*(Vector3d::dotProduct(impulse, velocity)/Vector3d::dotProduct(velocity,velocity));
            impulse *= -.5;

            std::cout << "Impulse: " << impulse.x << ", " << impulse.y << ", " << impulse.z << std::endl;
            m_currentVelocity = newVelocity;
        }
    }
}

const Omen::Transform& Camera::transform() const
{
    return m_transform;
}

Omen::Transform& Camera::transform()
{
    return m_transform;
}

/// EOF